#include "MvCameraControl.h"
// std
#include <cstdint>
#include <memory>
#include <filesystem>
#include <thread>
#include <queue>
#include <mutex>
#include <condition_variable>
#include <atomic>
// ROS
#include <camera_info_manager/camera_info_manager.hpp>
#include <image_transport/image_transport.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/utilities.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/detail/camera_info__struct.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <cv_bridge/cv_bridge.h>
// OpenCV
#include <opencv2/opencv.hpp>
// interfaces
#include <interfaces/msg/readpack.hpp>
#include <rm_interfaces/srv/set_camera_id.hpp>

namespace rm_camdrv {
class CamdrvNode : public rclcpp::Node {
public:
  explicit CamdrvNode(const rclcpp::NodeOptions &options);
  ~CamdrvNode() override;

private:
  void declareParameters();

  rcl_interfaces::msg::SetParametersResult
  parametersCallback(const std::vector<rclcpp::Parameter> &parameters);

  enum CAMMOD {
    UNINITIALIZED = -1,
    UNKNOWN = 0,

    AIMBOT = 1,

    SMALL_RUNE = 2,
    BIG_RUNE = 3,

    OUTPOST = 4,

    BASE = 5,

    HIT_FARAWAY = 6,
  };

  struct ModeParams {
    ModeParams()
        : camera_handle(nullptr),
          camera_info_msg(nullptr),
          camera_id(0),
          exposure_time(0.0),
          gain(0.0),
          auto_white_balance(0),
          balance_ratio_red(0),
          balance_ratio_green(0),
          balance_ratio_blue(0) {}

    void* camera_handle;
    std::shared_ptr<sensor_msgs::msg::CameraInfo> camera_info_msg;
    int camera_id;
    uint16_t width;
    uint16_t height;
    double exposure_time;
    double gain;
    uint16_t auto_white_balance; // 0: OFF, 1: CONTINUOUS, 2: ONCE
    uint16_t balance_ratio_red;
    uint16_t balance_ratio_green;
    uint16_t balance_ratio_blue;
  };

  std::unordered_map<std::string, std::unique_ptr<ModeParams>> mode_params_;

  template <typename T>
  struct SweepParams {
    SweepParams(T min_ = 0, T max_ = 0, T step_ = 0)
        : min(min_), max(max_), step(step_), current(min_) {}
    T min, max, step, current;

    // Return true when start from the beginning
    bool next() {
      current += step;
      if (current > max) {
        current = min;
        return true;
      }
      return false;
    }
    
    void reset() {
      current = min;
    }
  };
  SweepParams<double> exposure_time_sweep_;
  SweepParams<double> gain_sweep_;
  double sweep_interval_;

  void destroyCameraThread(void* handle);
  void createCameraThread(void* handle);
  void callCamInfoUpdater();
  void checkMode(const interfaces::msg::Readpack::SharedPtr msg);
  void setParams(const std::string &mode);
  
  // Parameter sweep thread
  void createParamSweepThread();

  // Release old recorder and create a new one
  void createRecorder();

  int last_mode_ = CAMMOD::UNINITIALIZED;

  sensor_msgs::msg::Image image_msg_;

  image_transport::CameraPublisher camera_pub_;

  rclcpp::Subscription<interfaces::msg::Readpack>::SharedPtr readpack_sub_;

  rclcpp::Client<rm_interfaces::srv::SetCameraID>::SharedPtr cam_info_updater_cli_;

  int camera_count_;
  std::string current_mode_;
  std::vector<void*> camera_handles_;
  std::vector<sensor_msgs::msg::CameraInfo> camera_info_msgs_;

  MV_CC_DEVICE_INFO_LIST device_list;

  int default_camera_id_;
  void* default_camera_handle_;

  std::unique_ptr<camera_info_manager::CameraInfoManager> camera_info_manager_;

  // Whether to keep the camera thread alive
  bool camera_thread_alive_;

  // Recorder
  bool record_;
  int max_frames_;
  int fps_;
  std::string record_path_;
  cv::VideoWriter video_writer_;
  
  // Recording frame rate control
  int frame_skip_interval_;  // Skip N frames then record 1 frame (e.g., 5 means record every 5th frame)
  int frame_skip_counter_;   // Counter for frame skipping
  
  // Async recording
  std::thread recording_thread_;
  std::queue<cv::Mat> frame_queue_;
  std::mutex queue_mutex_;
  std::condition_variable queue_condition_;
  bool recording_thread_alive_;
  std::atomic<int> frames_recorded_;

  // Sweep mode
  bool sweep_mode_;
  std::thread param_sweep_thread_;
  bool param_sweep_thread_alive_;

  int nRet = MV_OK;
  MV_IMAGE_BASIC_INFO img_info_;

  MV_CC_PIXEL_CONVERT_PARAM convert_param_;
  
  // Track if camera supports native RGB8 format
  bool use_native_rgb8_;
  bool prefer_native_rgb8_;

  int fail_conut_ = 0;
  std::thread capture_thread_;

  OnSetParametersCallbackHandle::SharedPtr params_callback_handle_;
};
} // namespace rm_camdrv
